/*
 * Copyright 2018-2019 Autoware Foundation. All rights reserved.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

/**
 * @file
 * Modified function input and used only some functions
 **/
#include "TrajectoryDynamicCosts.h"
#include "float.h"
#include "PlanningHelpers.h"

TrajectoryDynamicCosts::TrajectoryDynamicCosts()
{
    m_PrevCostIndex = -1;
    m_WeightLong = 1.0;
    m_WeightLat = 1.2;
    m_WeightLaneChange = 0.0;
    m_LateralSkipDistance = 50;

    m_CollisionTimeDiff = 6.0; // seconds
    m_PrevIndex = -1;
    m_WeightPriority = 0.9;
    m_WeightTransition = 0.9;
}

TrajectoryDynamicCosts::~TrajectoryDynamicCosts()
{
}

OP_TrajectoryCost TrajectoryDynamicCosts::DoOneStepDynamic(const vector<vector<WayPoint>> &rollOuts,
                                                           const vector<WayPoint> &totalPaths, const WayPoint &currState,
                                                           const PlanningParams &params, const CAR_BASIC_INFO &carInfo, const VehicleState &vehicleState,
                                                           const std::vector<DetectedObject> &obj_list, const int &iCurrentIndex)
{
    OP_TrajectoryCost bestTrajectory;
    bestTrajectory.bBlocked = true;
    bestTrajectory.closest_obj_distance = params.horizonDistance;
    bestTrajectory.closest_obj_velocity = 0;
    bestTrajectory.index = -1;

    double critical_lateral_distance = carInfo.width / 2.0 + params.horizontalSafetyDistancel;
    double critical_long_front_distance = carInfo.wheel_base / 2.0 + carInfo.length / 2.0 + params.verticalSafetyDistance;
    double critical_long_back_distance = carInfo.length / 2.0 + params.verticalSafetyDistance - carInfo.wheel_base / 2.0;

    int currIndex = -1;
    if (iCurrentIndex >= 0 && iCurrentIndex < rollOuts.size())
        currIndex = iCurrentIndex;
    else
        currIndex = GetCurrentRollOutIndex(totalPaths, currState, params);

    InitializeCosts(rollOuts, params);

    InitializeSafetyPolygon(currState, carInfo, vehicleState, critical_lateral_distance, critical_long_front_distance, critical_long_back_distance);

    CalculateTransitionCosts(m_TrajectoryCosts, currIndex, params);

    CalculateLateralAndLongitudinalCostsDynamic(obj_list, rollOuts, totalPaths, currState, params, carInfo, vehicleState, critical_lateral_distance, critical_long_front_distance, critical_long_back_distance);

    NormalizeCosts(m_TrajectoryCosts);

    int smallestIndex = -1;
    double smallestCost = DBL_MAX;
    double smallestDistance = DBL_MAX;
    double velo_of_next = 0;
    bool bAllFree = true;

    // cout << "Trajectory Costs Log : CurrIndex: " << currIndex << " --------------------- " << endl;
    for (unsigned int ic = 0; ic < m_TrajectoryCosts.size(); ic++)
    {
        // cout << m_TrajectoryCosts.at(ic).ToString();
        if (!m_TrajectoryCosts.at(ic).bBlocked && m_TrajectoryCosts.at(ic).cost < smallestCost)
        {
            smallestCost = m_TrajectoryCosts.at(ic).cost;
            smallestIndex = ic;
        }

        if (m_TrajectoryCosts.at(ic).closest_obj_distance < smallestDistance)
        {
            smallestDistance = m_TrajectoryCosts.at(ic).closest_obj_distance;
            velo_of_next = m_TrajectoryCosts.at(ic).closest_obj_velocity;
        }

        if (m_TrajectoryCosts.at(ic).bBlocked)
            bAllFree = false;
    }
    // cout << "Smallest Distance: " <<  smallestDistance << "------------------------------------------------------------- " << endl;

    if (bAllFree && smallestIndex >= 0)
        smallestIndex = params.rollOutNumber / 2;

    if (smallestIndex == -1)
    {
        bestTrajectory.bBlocked = true;
        bestTrajectory.lane_index = 0;
        bestTrajectory.index = m_PrevCostIndex;
        bestTrajectory.closest_obj_distance = smallestDistance;
        bestTrajectory.closest_obj_velocity = velo_of_next;
    }
    else if (smallestIndex >= 0)
    {
        bestTrajectory = m_TrajectoryCosts.at(smallestIndex);
    }

    m_PrevIndex = currIndex;

    // std::cout << "Current Selected Index : " << bestTrajectory.index << std::endl;
    return bestTrajectory;
}

OP_TrajectoryCost TrajectoryDynamicCosts::DoOneStepStatic(const vector<vector<WayPoint>> &rollOuts,
                                                          const vector<WayPoint> &totalPaths, const WayPoint &currState,
                                                          const PlanningParams &params, const CAR_BASIC_INFO &carInfo, const VehicleState &vehicleState,
                                                          const std::vector<DetectedObject> &obj_list, const int &iCurrentIndex)
{
    OP_TrajectoryCost bestTrajectory;
    bestTrajectory.bBlocked = true;
    bestTrajectory.closest_obj_distance = params.horizonDistance;
    bestTrajectory.closest_obj_velocity = 0;
    bestTrajectory.index = -1;

    RelativeInfo obj_info;
    PlanningHelpers::GetRelativeInfo(totalPaths, currState, obj_info);
    int currIndex = params.rollOutNumber / 2 + floor(obj_info.perp_distance / params.rollOutDensity);
    // std::cout <<  "Current Index: " << currIndex << std::endl;
    if (currIndex < 0)
        currIndex = 0;
    else if (currIndex > params.rollOutNumber)
        currIndex = params.rollOutNumber;

    m_TrajectoryCosts.clear();
    if (rollOuts.size() > 0)
    {
         // cal center cost
        OP_TrajectoryCost tc;
        int centralIndex = params.rollOutNumber / 2;
        tc.lane_index = 0;
        for (unsigned int it = 0; it < rollOuts.size(); it++)
        {
            tc.index = it;
            tc.relative_index = it - centralIndex;
            tc.distance_from_center = params.rollOutDensity * tc.relative_index;
            tc.priority_cost = fabs(tc.distance_from_center);
            tc.closest_obj_distance = params.horizonDistance;
            if (rollOuts.at(it).size() > 0)
                tc.lane_change_cost = rollOuts.at(it).at(0).laneChangeCost;
            m_TrajectoryCosts.push_back(tc);
        }
    }

    // cal transition cost
    CalculateTransitionCosts(m_TrajectoryCosts, currIndex, params);

    WayPoint p;
    m_AllContourPoints.clear();
    for (unsigned int io = 0; io < obj_list.size(); io++)
    {
        for (unsigned int icon = 0; icon < obj_list.at(io).contour.size(); icon++)
        {
            p.pos = obj_list.at(io).contour.at(icon);
            p.v = obj_list.at(io).center.v;
            p.id = io;
            p.cost = sqrt(obj_list.at(io).w * obj_list.at(io).w + obj_list.at(io).l * obj_list.at(io).l);
            m_AllContourPoints.push_back(p);
        }
    }

    // cal lateral and longitudinal cost
    CalculateLateralAndLongitudinalCostsStatic(m_TrajectoryCosts, rollOuts, totalPaths, currState, m_AllContourPoints, params, carInfo, vehicleState);

    NormalizeCosts(m_TrajectoryCosts);

    int smallestIndex = -1;
    double smallestCost = DBL_MAX;
    double smallestDistance = DBL_MAX;
    double velo_of_next = 0;

    // cout << "Trajectory Costs Log : CurrIndex: " << currIndex << " --------------------- " << endl;
    for (unsigned int ic = 0; ic < m_TrajectoryCosts.size(); ic++)
    {
        // cout << m_TrajectoryCosts.at(ic).ToString();
        if (!m_TrajectoryCosts.at(ic).bBlocked && m_TrajectoryCosts.at(ic).cost < smallestCost)
        {
            smallestCost = m_TrajectoryCosts.at(ic).cost;
            smallestIndex = ic;
        }

        if (m_TrajectoryCosts.at(ic).closest_obj_distance < smallestDistance)
        {
            smallestDistance = m_TrajectoryCosts.at(ic).closest_obj_distance;
            velo_of_next = m_TrajectoryCosts.at(ic).closest_obj_velocity;
        }
    }
    // cout << "Smallest Distance: " <<  smallestDistance << "------------------------------------------------------------- " << endl;

    if (smallestIndex == -1)
    {
        bestTrajectory.bBlocked = true;
        bestTrajectory.lane_index = 0;
        bestTrajectory.index = m_PrevCostIndex;
        bestTrajectory.closest_obj_distance = smallestDistance;
        bestTrajectory.closest_obj_velocity = velo_of_next;
    }
    else if (smallestIndex >= 0)
    {
        bestTrajectory = m_TrajectoryCosts.at(smallestIndex);
    }

    m_PrevIndex = currIndex;
    return bestTrajectory;
}

OP_TrajectoryCost TrajectoryDynamicCosts::DoOneStep(const vector<vector<vector<WayPoint>>> &rollOuts,
                                                    const vector<vector<WayPoint>> &totalPaths, const WayPoint &currState, const int &currIndex,
                                                    const int &currLaneIndex,
                                                    const PlanningParams &params, const CAR_BASIC_INFO &carInfo, const VehicleState &vehicleState,
                                                    const std::vector<DetectedObject> &obj_list)
{
    OP_TrajectoryCost bestTrajectory;
    bestTrajectory.bBlocked = true;
    bestTrajectory.closest_obj_distance = params.horizonDistance;
    bestTrajectory.closest_obj_velocity = 0;
    bestTrajectory.index = -1;

    if (!ValidateRollOutsInput(rollOuts) || rollOuts.size() != totalPaths.size())
        return bestTrajectory;

    if (m_PrevCostIndex == -1)
        m_PrevCostIndex = params.rollOutNumber / 2;

    m_TrajectoryCosts.clear();

    for (unsigned int il = 0; il < rollOuts.size(); il++)
    {
        if (rollOuts.at(il).size() > 0 && rollOuts.at(il).at(0).size() > 0)
        {
            vector<OP_TrajectoryCost> costs = CalculatePriorityAndLaneChangeCosts(rollOuts.at(il), il, params);
            m_TrajectoryCosts.insert(m_TrajectoryCosts.end(), costs.begin(), costs.end());
        }
    }

    CalculateTransitionCosts(m_TrajectoryCosts, currIndex, params);

    WayPoint p;
    m_AllContourPoints.clear();
    for (unsigned int io = 0; io < obj_list.size(); io++)
    {
        for (unsigned int icon = 0; icon < obj_list.at(io).contour.size(); icon++)
        {
            p.pos = obj_list.at(io).contour.at(icon);
            p.v = obj_list.at(io).center.v;
            p.id = io;
            p.cost = sqrt(obj_list.at(io).w * obj_list.at(io).w + obj_list.at(io).l * obj_list.at(io).l);
            m_AllContourPoints.push_back(p);
        }
    }

    CalculateLateralAndLongitudinalCosts(m_TrajectoryCosts, rollOuts, totalPaths, currState, m_AllContourPoints, params, carInfo, vehicleState);

    NormalizeCosts(m_TrajectoryCosts);

    int smallestIndex = -1;
    double smallestCost = DBL_MAX;
    double smallestDistance = DBL_MAX;
    double velo_of_next = 0;

    // cout << "Trajectory Costs Log : CurrIndex: " << currIndex << " --------------------- " << endl;
    for (unsigned int ic = 0; ic < m_TrajectoryCosts.size(); ic++)
    {
        // cout << m_TrajectoryCosts.at(ic).ToString();
        if (!m_TrajectoryCosts.at(ic).bBlocked && m_TrajectoryCosts.at(ic).cost < smallestCost)
        {
            smallestCost = m_TrajectoryCosts.at(ic).cost;
            smallestIndex = ic;
        }

        if (m_TrajectoryCosts.at(ic).closest_obj_distance < smallestDistance)
        {
            smallestDistance = m_TrajectoryCosts.at(ic).closest_obj_distance;
            velo_of_next = m_TrajectoryCosts.at(ic).closest_obj_velocity;
        }
    }

    // cout << "Smallest Distance: " <<  smallestDistance << "------------------------------------------------------------- " << endl;

    // All is blocked !
    if (smallestIndex == -1 && m_PrevCostIndex < (int)m_TrajectoryCosts.size())
    {
        bestTrajectory.bBlocked = true;
        bestTrajectory.lane_index = currLaneIndex;
        bestTrajectory.index = currIndex;
        bestTrajectory.closest_obj_distance = smallestDistance;
        bestTrajectory.closest_obj_velocity = velo_of_next;
        // bestTrajectory.index = smallestIndex;
    }
    else if (smallestIndex >= 0)
    {
        bestTrajectory = m_TrajectoryCosts.at(smallestIndex);
        // bestTrajectory.index = smallestIndex;
    }

    //  cout << "smallestI: " <<  smallestIndex << ", C_Size: " << m_TrajectoryCosts.size()
    //      << ", LaneI: " << bestTrajectory.lane_index << "TrajI: " << bestTrajectory.index
    //      << ", prevSmalI: " << m_PrevCostIndex << ", distance: " << bestTrajectory.closest_obj_distance
    //      << ", Blocked: " << bestTrajectory.bBlocked
    //      << endl;

    m_PrevCostIndex = smallestIndex;

    return bestTrajectory;
}

void TrajectoryDynamicCosts::CalculateLateralAndLongitudinalCostsStatic(vector<OP_TrajectoryCost> &trajectoryCosts,
                                                                        const vector<vector<WayPoint>> &rollOuts, const vector<WayPoint> &totalPaths,
                                                                        const WayPoint &currState, const vector<WayPoint> &contourPoints, const PlanningParams &params,
                                                                        const CAR_BASIC_INFO &carInfo, const VehicleState &vehicleState)
{
    double critical_lateral_distance = carInfo.width / 2.0 + params.horizontalSafetyDistancel;
    double critical_long_front_distance = carInfo.wheel_base / 2.0 + carInfo.length / 2.0 + params.verticalSafetyDistance;
    double critical_long_back_distance = carInfo.length / 2.0 + params.verticalSafetyDistance - carInfo.wheel_base / 2.0;

    Mat3 invRotationMat(currState.pos.a - M_PI_2);
    Mat3 invTranslationMat(currState.pos.x, currState.pos.y);

    double corner_slide_distance = critical_lateral_distance / 2.0;
    double ratio_to_angle = corner_slide_distance / carInfo.max_steer_angle;
    double slide_distance = vehicleState.steer * ratio_to_angle;

    GpsPoint bottom_left(-critical_lateral_distance, -critical_long_back_distance, currState.pos.z, 0);
    GpsPoint bottom_right(critical_lateral_distance, -critical_long_back_distance, currState.pos.z, 0);

    GpsPoint top_right_car(critical_lateral_distance, carInfo.wheel_base / 3.0 + carInfo.length / 3.0, currState.pos.z, 0);
    GpsPoint top_left_car(-critical_lateral_distance, carInfo.wheel_base / 3.0 + carInfo.length / 3.0, currState.pos.z, 0);

    GpsPoint top_right(critical_lateral_distance - slide_distance, critical_long_front_distance, currState.pos.z, 0);
    GpsPoint top_left(-critical_lateral_distance - slide_distance, critical_long_front_distance, currState.pos.z, 0);

    bottom_left = invRotationMat * bottom_left;
    bottom_left = invTranslationMat * bottom_left;

    top_right = invRotationMat * top_right;
    top_right = invTranslationMat * top_right;

    bottom_right = invRotationMat * bottom_right;
    bottom_right = invTranslationMat * bottom_right;

    top_left = invRotationMat * top_left;
    top_left = invTranslationMat * top_left;

    top_right_car = invRotationMat * top_right_car;
    top_right_car = invTranslationMat * top_right_car;

    top_left_car = invRotationMat * top_left_car;
    top_left_car = invTranslationMat * top_left_car;

    m_SafetyBorder.points.clear();
    m_SafetyBorder.points.push_back(bottom_left);
    m_SafetyBorder.points.push_back(bottom_right);
    m_SafetyBorder.points.push_back(top_right_car);
    m_SafetyBorder.points.push_back(top_right);
    m_SafetyBorder.points.push_back(top_left);
    m_SafetyBorder.points.push_back(top_left_car);

    int iCostIndex = 0;
    if (rollOuts.size() > 0 && rollOuts.at(0).size() > 0)
    {
        RelativeInfo car_info;
        PlanningHelpers::GetRelativeInfo(totalPaths, currState, car_info);

        for (unsigned int it = 0; it < rollOuts.size(); it++)
        {
            int skip_id = -1;
            for (unsigned int icon = 0; icon < contourPoints.size(); icon++)
            {
                if (skip_id == contourPoints.at(icon).id)
                    continue;

                RelativeInfo obj_info;
                PlanningHelpers::GetRelativeInfo(totalPaths, contourPoints.at(icon), obj_info);
                double longitudinalDist = PlanningHelpers::GetExactDistanceOnTrajectory(totalPaths, car_info, obj_info);
                if (obj_info.iFront == 0 && longitudinalDist > 0)
                    longitudinalDist = -longitudinalDist;
                // 计算当前障碍物点到车辆位置的沿着中心轨迹的距离
                double direct_distance = hypot(obj_info.perp_point.pos.y - contourPoints.at(icon).pos.y, obj_info.perp_point.pos.x - contourPoints.at(icon).pos.x);
                if (contourPoints.at(icon).v < params.minSpeed && direct_distance > (m_LateralSkipDistance + contourPoints.at(icon).cost))
                {
                    skip_id = contourPoints.at(icon).id;
                    continue;
                }

                double close_in_percentage = 1;
                // close_in_percentage = ((longitudinalDist - critical_long_front_distance) / params.rollInMargin) * 4.0;

                // if (close_in_percentage <= 0 || close_in_percentage > 1)
                //     close_in_percentage = 1;

                double distance_from_center = trajectoryCosts.at(iCostIndex).distance_from_center;

                if (close_in_percentage < 1)
                    distance_from_center = distance_from_center - distance_from_center * (1.0 - close_in_percentage);

                double lateralDist = fabs(obj_info.perp_distance - distance_from_center);

                if (longitudinalDist < -carInfo.length || longitudinalDist > params.minFollowingDistance || lateralDist > m_LateralSkipDistance)
                {
                    continue;
                }

                longitudinalDist = longitudinalDist - critical_long_front_distance;

                if (m_SafetyBorder.PointInsidePolygon(m_SafetyBorder, contourPoints.at(icon).pos) == true)
                    trajectoryCosts.at(iCostIndex).bBlocked = true;

                if (lateralDist <= critical_lateral_distance && longitudinalDist >= -carInfo.length / 1.5 && longitudinalDist < params.minFollowingDistance)
                    trajectoryCosts.at(iCostIndex).bBlocked = true;

                if (lateralDist != 0)
                    trajectoryCosts.at(iCostIndex).lateral_cost += 1.0 / lateralDist;

                if (longitudinalDist != 0)
                    trajectoryCosts.at(iCostIndex).longitudinal_cost += 1.0 / fabs(longitudinalDist);

                if (longitudinalDist >= -critical_long_front_distance && longitudinalDist < trajectoryCosts.at(iCostIndex).closest_obj_distance)
                {
                    trajectoryCosts.at(iCostIndex).closest_obj_distance = longitudinalDist;
                    trajectoryCosts.at(iCostIndex).closest_obj_velocity = contourPoints.at(icon).v;
                }
            }

            iCostIndex++;
        }
    }
}

void TrajectoryDynamicCosts::CalculateLateralAndLongitudinalCosts(vector<OP_TrajectoryCost> &trajectoryCosts,
                                                                  const vector<vector<vector<WayPoint>>> &rollOuts, const vector<vector<WayPoint>> &totalPaths,
                                                                  const WayPoint &currState, const vector<WayPoint> &contourPoints, const PlanningParams &params,
                                                                  const CAR_BASIC_INFO &carInfo, const VehicleState &vehicleState)
{
    double critical_lateral_distance = carInfo.width / 2.0 + params.horizontalSafetyDistancel;
    double critical_long_front_distance = carInfo.wheel_base / 2.0 + carInfo.length / 2.0 + params.verticalSafetyDistance;
    double critical_long_back_distance = carInfo.length / 2.0 + params.verticalSafetyDistance - carInfo.wheel_base / 2.0;
    int iCostIndex = 0;

    Mat3 invRotationMat(currState.pos.a - M_PI_2);
    Mat3 invTranslationMat(currState.pos.x, currState.pos.y);

    double corner_slide_distance = critical_lateral_distance / 2.0;
    double ratio_to_angle = corner_slide_distance / carInfo.max_steer_angle;
    double slide_distance = vehicleState.steer * ratio_to_angle;

    GpsPoint bottom_left(-critical_lateral_distance, -critical_long_back_distance, currState.pos.z, 0);
    GpsPoint bottom_right(critical_lateral_distance, -critical_long_back_distance, currState.pos.z, 0);

    GpsPoint top_right_car(critical_lateral_distance, carInfo.wheel_base / 3.0 + carInfo.length / 3.0, currState.pos.z, 0);
    GpsPoint top_left_car(-critical_lateral_distance, carInfo.wheel_base / 3.0 + carInfo.length / 3.0, currState.pos.z, 0);

    GpsPoint top_right(critical_lateral_distance - slide_distance, critical_long_front_distance, currState.pos.z, 0);
    GpsPoint top_left(-critical_lateral_distance - slide_distance, critical_long_front_distance, currState.pos.z, 0);

    bottom_left = invRotationMat * bottom_left;
    bottom_left = invTranslationMat * bottom_left;

    top_right = invRotationMat * top_right;
    top_right = invTranslationMat * top_right;

    bottom_right = invRotationMat * bottom_right;
    bottom_right = invTranslationMat * bottom_right;

    top_left = invRotationMat * top_left;
    top_left = invTranslationMat * top_left;

    top_right_car = invRotationMat * top_right_car;
    top_right_car = invTranslationMat * top_right_car;

    top_left_car = invRotationMat * top_left_car;
    top_left_car = invTranslationMat * top_left_car;

    m_SafetyBorder.points.clear();
    m_SafetyBorder.points.push_back(bottom_left);
    m_SafetyBorder.points.push_back(bottom_right);
    m_SafetyBorder.points.push_back(top_right_car);
    m_SafetyBorder.points.push_back(top_right);
    m_SafetyBorder.points.push_back(top_left);
    m_SafetyBorder.points.push_back(top_left_car);

    for (unsigned int il = 0; il < rollOuts.size(); il++)
    {
        if (rollOuts.at(il).size() > 0 && rollOuts.at(il).at(0).size() > 0)
        {
            RelativeInfo car_info;
            PlanningHelpers::GetRelativeInfo(totalPaths.at(il), currState, car_info);

            for (unsigned int it = 0; it < rollOuts.at(il).size(); it++)
            {
                int skip_id = -1;
                for (unsigned int icon = 0; icon < contourPoints.size(); icon++)
                {
                    if (skip_id == contourPoints.at(icon).id)
                        continue;

                    RelativeInfo obj_info;
                    PlanningHelpers::GetRelativeInfo(totalPaths.at(il), contourPoints.at(icon), obj_info);
                    double longitudinalDist = PlanningHelpers::GetExactDistanceOnTrajectory(totalPaths.at(il), car_info, obj_info);
                    if (obj_info.iFront == 0 && longitudinalDist > 0)
                        longitudinalDist = -longitudinalDist;

                    double direct_distance = hypot(obj_info.perp_point.pos.y - contourPoints.at(icon).pos.y, obj_info.perp_point.pos.x - contourPoints.at(icon).pos.x);
                    if (contourPoints.at(icon).v < params.minSpeed && direct_distance > (m_LateralSkipDistance + contourPoints.at(icon).cost))
                    {
                        skip_id = contourPoints.at(icon).id;
                        continue;
                    }

                    double close_in_percentage = 1;
                    //          close_in_percentage = ((longitudinalDist- critical_long_front_distance)/params.rollInMargin)*4.0;
                    //
                    //          if(close_in_percentage <= 0 || close_in_percentage > 1) close_in_percentage = 1;

                    double distance_from_center = trajectoryCosts.at(iCostIndex).distance_from_center;

                    if (close_in_percentage < 1)
                        distance_from_center = distance_from_center - distance_from_center * (1.0 - close_in_percentage);

                    double lateralDist = fabs(obj_info.perp_distance - distance_from_center);

                    if (longitudinalDist < -carInfo.length || longitudinalDist > params.minFollowingDistance || lateralDist > m_LateralSkipDistance)
                    {
                        continue;
                    }

                    longitudinalDist = longitudinalDist - critical_long_front_distance;

                    if (m_SafetyBorder.PointInsidePolygon(m_SafetyBorder, contourPoints.at(icon).pos) == true)
                        trajectoryCosts.at(iCostIndex).bBlocked = true;

                    if (lateralDist <= critical_lateral_distance && longitudinalDist >= -carInfo.length / 1.5 && longitudinalDist < params.minFollowingDistance)
                        trajectoryCosts.at(iCostIndex).bBlocked = true;

                    if (lateralDist != 0)
                        trajectoryCosts.at(iCostIndex).lateral_cost += 1.0 / lateralDist;

                    if (longitudinalDist != 0)
                        trajectoryCosts.at(iCostIndex).longitudinal_cost += 1.0 / fabs(longitudinalDist);

                    if (longitudinalDist >= -critical_long_front_distance && longitudinalDist < trajectoryCosts.at(iCostIndex).closest_obj_distance)
                    {
                        trajectoryCosts.at(iCostIndex).closest_obj_distance = longitudinalDist;
                        trajectoryCosts.at(iCostIndex).closest_obj_velocity = contourPoints.at(icon).v;
                    }
                }

                iCostIndex++;
            }
        }
    }
}

void TrajectoryDynamicCosts::NormalizeCosts(vector<OP_TrajectoryCost> &trajectoryCosts)
{
    // Normalize costs
    double totalPriorities = 0;
    double totalChange = 0;
    double totalLateralCosts = 0;
    double totalLongitudinalCosts = 0;
    double transitionCosts = 0;

    for (unsigned int ic = 0; ic < trajectoryCosts.size(); ic++)
    {
        totalPriorities += trajectoryCosts.at(ic).priority_cost;
        transitionCosts += trajectoryCosts.at(ic).transition_cost;
    }

    for (unsigned int ic = 0; ic < trajectoryCosts.size(); ic++)
    {
        totalChange += trajectoryCosts.at(ic).lane_change_cost;
        totalLateralCosts += trajectoryCosts.at(ic).lateral_cost;
        totalLongitudinalCosts += trajectoryCosts.at(ic).longitudinal_cost;
    }

    //  cout << "------ Normalizing Step " << endl;
    for (unsigned int ic = 0; ic < trajectoryCosts.size(); ic++)
    {
        if (totalPriorities != 0)
            trajectoryCosts.at(ic).priority_cost = trajectoryCosts.at(ic).priority_cost / totalPriorities;
        else
            trajectoryCosts.at(ic).priority_cost = 0;

        if (transitionCosts != 0)
            trajectoryCosts.at(ic).transition_cost = trajectoryCosts.at(ic).transition_cost / transitionCosts;
        else
            trajectoryCosts.at(ic).transition_cost = 0;

        if (totalChange != 0)
            trajectoryCosts.at(ic).lane_change_cost = trajectoryCosts.at(ic).lane_change_cost / totalChange;
        else
            trajectoryCosts.at(ic).lane_change_cost = 0;

        if (totalLateralCosts != 0)
            trajectoryCosts.at(ic).lateral_cost = trajectoryCosts.at(ic).lateral_cost / totalLateralCosts;
        else
            trajectoryCosts.at(ic).lateral_cost = 0;

        if (totalLongitudinalCosts != 0)
            trajectoryCosts.at(ic).longitudinal_cost = trajectoryCosts.at(ic).longitudinal_cost / totalLongitudinalCosts;
        else
            trajectoryCosts.at(ic).longitudinal_cost = 0;

        trajectoryCosts.at(ic).cost = (m_WeightPriority * trajectoryCosts.at(ic).priority_cost + m_WeightTransition * trajectoryCosts.at(ic).transition_cost + m_WeightLat * trajectoryCosts.at(ic).lateral_cost + m_WeightLong * trajectoryCosts.at(ic).longitudinal_cost) / 4.0;

        //    cout << "Index: " << ic
        //            << ", Priority: " << trajectoryCosts.at(ic).priority_cost
        //            << ", Transition: " << trajectoryCosts.at(ic).transition_cost
        //            << ", Lat: " << trajectoryCosts.at(ic).lateral_cost
        //            << ", Long: " << trajectoryCosts.at(ic).longitudinal_cost
        //            << ", Change: " << trajectoryCosts.at(ic).lane_change_cost
        //            << ", Avg: " << trajectoryCosts.at(ic).cost
        //            << endl;
    }

    //  cout << "------------------------ " << endl;
}

vector<OP_TrajectoryCost> TrajectoryDynamicCosts::CalculatePriorityAndLaneChangeCosts(const vector<vector<WayPoint>> &laneRollOuts,
                                                                                      const int &lane_index, const PlanningParams &params)
{
    vector<OP_TrajectoryCost> costs;
    OP_TrajectoryCost tc;
    int centralIndex = params.rollOutNumber / 2;

    tc.lane_index = lane_index;
    for (unsigned int it = 0; it < laneRollOuts.size(); it++)
    {
        tc.index = it;
        tc.relative_index = it - centralIndex;
        tc.distance_from_center = params.rollOutDensity * tc.relative_index;
        tc.priority_cost = fabs(tc.distance_from_center);
        tc.closest_obj_distance = params.horizonDistance;
        tc.lane_change_cost = laneRollOuts.at(it).at(0).laneChangeCost;

        //    if(laneRollOuts.at(it).at(0).bDir == FORWARD_LEFT_DIR || laneRollOuts.at(it).at(0).bDir == FORWARD_RIGHT_DIR)
        //      tc.lane_change_cost = 1;
        //    else if(laneRollOuts.at(it).at(0).bDir == BACKWARD_DIR || laneRollOuts.at(it).at(0).bDir == BACKWARD_RIGHT_DIR || laneRollOuts.at(it).at(0).bDir == BACKWARD_LEFT_DIR)
        //      tc.lane_change_cost = 2;
        //    else
        //      tc.lane_change_cost = 0;

        costs.push_back(tc);
    }

    return costs;
}

void TrajectoryDynamicCosts::CalculateTransitionCosts(vector<OP_TrajectoryCost> &trajectoryCosts, const int &currTrajectoryIndex, const PlanningParams &params)
{
    for (int ic = 0; ic < trajectoryCosts.size(); ic++)
    {
        trajectoryCosts.at(ic).transition_cost = fabs(params.rollOutDensity * (ic - currTrajectoryIndex));
    }
}

/**
 * @brief Validate input, each trajectory must have at least 1 way point
 * @param rollOuts
 * @return true if data isvalid for cost calculation
 */
bool TrajectoryDynamicCosts::ValidateRollOutsInput(const vector<vector<vector<WayPoint>>> &rollOuts)
{
    if (rollOuts.size() == 0)
        return false;

    for (unsigned int il = 0; il < rollOuts.size(); il++)
    {
        if (rollOuts.at(il).size() == 0)
            return false;
    }

    return true;
}

void TrajectoryDynamicCosts::CalculateIntersectionVelocities(const std::vector<WayPoint> &path, const DetectedObject &obj, const WayPoint &currPose, const CAR_BASIC_INFO &carInfo, const double &c_lateral_d, WayPoint &collisionPoint, OP_TrajectoryCost &trajectoryCosts)
{
    trajectoryCosts.bBlocked = false;
    int closest_path_i = path.size();
    for (unsigned int k = 0; k < obj.predTrajectories.size(); k++)
    {
        for (unsigned int j = 0; j < obj.predTrajectories.at(k).size(); j++)
        {
            for (unsigned int i = 0; i < path.size(); i++)
            {
                // if(path.at(i).timeCost > -1)
                {
                    double collision_distance = hypot(path.at(i).pos.x - obj.predTrajectories.at(k).at(j).pos.x, path.at(i).pos.y - obj.predTrajectories.at(k).at(j).pos.y);
                    double collision_t = fabs(path.at(i).timeCost - obj.predTrajectories.at(k).at(j).timeCost);

                    // if(collision_distance <= c_lateral_d && i < closest_path_i && collision_t < m_CollisionTimeDiff)
                    if (collision_distance <= c_lateral_d && i < closest_path_i)
                    {

                        closest_path_i = i;
                        double a = PlanningHelpers::AngleBetweenTwoAnglesPositive(path.at(i).pos.a, obj.predTrajectories.at(k).at(j).pos.a) / M_PI;
                        if (a < 0.25 && (currPose.v - obj.center.v) > 0)
                            trajectoryCosts.closest_obj_velocity = (currPose.v - obj.center.v);
                        else
                            trajectoryCosts.closest_obj_velocity = currPose.v;

                        collisionPoint = path.at(i);
                        collisionPoint.collisionCost = collision_t;
                        collisionPoint.cost = collision_distance;
                        trajectoryCosts.bBlocked = true;
                    }
                }
            }
        }
    }
}

int TrajectoryDynamicCosts::GetCurrentRollOutIndex(const std::vector<WayPoint> &path, const WayPoint &currState, const PlanningParams &params)
{
    RelativeInfo obj_info;
    PlanningHelpers::GetRelativeInfo(path, currState, obj_info);
    int currIndex = params.rollOutNumber / 2 + floor(obj_info.perp_distance / params.rollOutDensity);
    if (currIndex < 0)
        currIndex = 0;
    else if (currIndex > params.rollOutNumber)
        currIndex = params.rollOutNumber;

    return currIndex;
}

void TrajectoryDynamicCosts::InitializeCosts(const vector<vector<WayPoint>> &rollOuts, const PlanningParams &params)
{
    m_TrajectoryCosts.clear();
    if (rollOuts.size() > 0)
    {
        OP_TrajectoryCost tc;
        int centralIndex = params.rollOutNumber / 2;
        tc.lane_index = 0;
        for (unsigned int it = 0; it < rollOuts.size(); it++)
        {
            tc.index = it;
            tc.relative_index = it - centralIndex;
            tc.distance_from_center = params.rollOutDensity * tc.relative_index;
            tc.priority_cost = fabs(tc.distance_from_center);
            tc.closest_obj_distance = params.horizonDistance;
            if (rollOuts.at(it).size() > 0)
                tc.lane_change_cost = rollOuts.at(it).at(0).laneChangeCost;
            m_TrajectoryCosts.push_back(tc);
        }
    }
}

void TrajectoryDynamicCosts::InitializeSafetyPolygon(const WayPoint &currState, const CAR_BASIC_INFO &carInfo, const VehicleState &vehicleState, const double &c_lateral_d, const double &c_long_front_d, const double &c_long_back_d)
{
    Mat3 invRotationMat(currState.pos.a - M_PI_2);
    Mat3 invTranslationMat(currState.pos.x, currState.pos.y);

    double corner_slide_distance = c_lateral_d / 2.0;
    double ratio_to_angle = corner_slide_distance / carInfo.max_steer_angle;
    double slide_distance = vehicleState.steer * ratio_to_angle;

    GpsPoint bottom_left(-c_lateral_d, -c_long_back_d, currState.pos.z, 0);
    GpsPoint bottom_right(c_lateral_d, -c_long_back_d, currState.pos.z, 0);

    GpsPoint top_right_car(c_lateral_d, carInfo.wheel_base / 3.0 + carInfo.length / 3.0, currState.pos.z, 0);
    GpsPoint top_left_car(-c_lateral_d, carInfo.wheel_base / 3.0 + carInfo.length / 3.0, currState.pos.z, 0);

    GpsPoint top_right(c_lateral_d - slide_distance, c_long_front_d, currState.pos.z, 0);
    GpsPoint top_left(-c_lateral_d - slide_distance, c_long_front_d, currState.pos.z, 0);

    bottom_left = invRotationMat * bottom_left;
    bottom_left = invTranslationMat * bottom_left;

    top_right = invRotationMat * top_right;
    top_right = invTranslationMat * top_right;

    bottom_right = invRotationMat * bottom_right;
    bottom_right = invTranslationMat * bottom_right;

    top_left = invRotationMat * top_left;
    top_left = invTranslationMat * top_left;

    top_right_car = invRotationMat * top_right_car;
    top_right_car = invTranslationMat * top_right_car;

    top_left_car = invRotationMat * top_left_car;
    top_left_car = invTranslationMat * top_left_car;

    m_SafetyBorder.points.clear();
    m_SafetyBorder.points.push_back(bottom_left);
    m_SafetyBorder.points.push_back(bottom_right);
    m_SafetyBorder.points.push_back(top_right_car);
    m_SafetyBorder.points.push_back(top_right);
    m_SafetyBorder.points.push_back(top_left);
    m_SafetyBorder.points.push_back(top_left_car);
}

void TrajectoryDynamicCosts::CalculateLateralAndLongitudinalCostsDynamic(const std::vector<DetectedObject> &obj_list, const vector<vector<WayPoint>> &rollOuts, const vector<WayPoint> &totalPaths,
                                                                         const WayPoint &currState, const PlanningParams &params, const CAR_BASIC_INFO &carInfo,
                                                                         const VehicleState &vehicleState, const double &c_lateral_d, const double &c_long_front_d, const double &c_long_back_d)
{

    RelativeInfo car_info;
    PlanningHelpers::GetRelativeInfo(totalPaths, currState, car_info);
    m_CollisionPoints.clear();

    for (unsigned int i = 0; i < obj_list.size(); i++)
    {
        if (obj_list.at(i).label.compare("curb") == 0)
        {
            double d = hypot(obj_list.at(i).center.pos.y - currState.pos.y, obj_list.at(i).center.pos.x - currState.pos.x);
            if (d > params.minFollowingDistance + c_lateral_d)
                continue;
        }

        if (obj_list.at(i).bVelocity && obj_list.at(i).predTrajectories.size() > 0) // dynamic
        {

            for (unsigned int ir = 0; ir < rollOuts.size(); ir++)
            {
                WayPoint collisionPoint;
                OP_TrajectoryCost trajectoryCosts;
                CalculateIntersectionVelocities(rollOuts.at(ir), obj_list.at(i), currState, carInfo, c_lateral_d, collisionPoint, trajectoryCosts);
                if (trajectoryCosts.bBlocked)
                {
                    RelativeInfo col_info;
                    PlanningHelpers::GetRelativeInfo(totalPaths, collisionPoint, col_info);
                    double longitudinalDist = PlanningHelpers::GetExactDistanceOnTrajectory(totalPaths, car_info, col_info);

                    if (col_info.iFront == 0 && longitudinalDist > 0)
                        longitudinalDist = -longitudinalDist;

                    if (longitudinalDist < -carInfo.length || longitudinalDist > params.minFollowingDistance || fabs(longitudinalDist) < carInfo.width / 2.0)
                        continue;

                    // std::cout << "LongDistance: " << longitudinalDist << std::endl;

                    if (longitudinalDist >= -c_long_front_d && longitudinalDist < m_TrajectoryCosts.at(ir).closest_obj_distance)
                        m_TrajectoryCosts.at(ir).closest_obj_distance = longitudinalDist;

                    m_TrajectoryCosts.at(ir).closest_obj_velocity = trajectoryCosts.closest_obj_velocity;
                    m_TrajectoryCosts.at(ir).bBlocked = true;

                    m_CollisionPoints.push_back(collisionPoint);
                }
            }
        }
        else
        {
            RelativeInfo obj_info;
            WayPoint corner_p;
            for (unsigned int icon = 0; icon < obj_list.at(i).contour.size(); icon++)
            {
                if (m_SafetyBorder.PointInsidePolygon(m_SafetyBorder, obj_list.at(i).contour.at(icon)) == true)
                {
                    for (unsigned int it = 0; it < rollOuts.size(); it++)
                        m_TrajectoryCosts.at(it).bBlocked = true;

                    return;
                }

                corner_p.pos = obj_list.at(i).contour.at(icon);
                PlanningHelpers::GetRelativeInfo(totalPaths, corner_p, obj_info);
                double longitudinalDist = PlanningHelpers::GetExactDistanceOnTrajectory(totalPaths, car_info, obj_info);
                if (obj_info.iFront == 0 && longitudinalDist > 0)
                    longitudinalDist = -longitudinalDist;

                if (longitudinalDist < -carInfo.length || longitudinalDist > params.minFollowingDistance)
                    continue;

                longitudinalDist = longitudinalDist - c_long_front_d;

                for (unsigned int it = 0; it < rollOuts.size(); it++)
                {
                    double lateralDist = fabs(obj_info.perp_distance - m_TrajectoryCosts.at(it).distance_from_center);

                    if (lateralDist > m_LateralSkipDistance)
                        continue;

                    if (lateralDist <= c_lateral_d && longitudinalDist > -carInfo.length && longitudinalDist < params.minFollowingDistance)
                    {
                        m_TrajectoryCosts.at(it).bBlocked = true;
                        m_CollisionPoints.push_back(obj_info.perp_point);
                    }

                    if (lateralDist != 0)
                        m_TrajectoryCosts.at(it).lateral_cost += 1.0 / lateralDist;

                    if (longitudinalDist != 0)
                        m_TrajectoryCosts.at(it).longitudinal_cost += 1.0 / fabs(longitudinalDist);

                    if (longitudinalDist >= -c_long_front_d && longitudinalDist < m_TrajectoryCosts.at(it).closest_obj_distance)
                    {
                        m_TrajectoryCosts.at(it).closest_obj_distance = longitudinalDist;
                        m_TrajectoryCosts.at(it).closest_obj_velocity = obj_list.at(i).center.v;
                    }
                }
            }
        }
    }
}